#include "scene.h"
#include "Bullet/bulletengine.h"
#include "rigidbody.h"
#include "rigidbodyengine.h"
#include <QTime>
#include <QtXml>
#include <fstream>
#include <QMessageBox>
#include <QFileDialog>


Scene::Scene()
{
    rootDir = "../../../../../svn/";

    engine = new BulletRigidBodyEngine();

    engine->pause = true;

    engine->timestep = 0.01;

    engine->setGravity(-10.0);

    selectedBody = NULL;
    timePhysics = 0;

    resourceManager = ResourceManager::getInstance();

    addGround();

    aabb.extend(ground->aabb);

    std::ifstream lastSavedFile(QString(rootDir + "scenes/lastSavedFile.txt").toStdString().c_str());

    std::string filename;
    lastSavedFile >> filename;
    lastSavedFile.close();
    load(QString(filename.c_str()));
}

void Scene::simulate(void)
{
    QTime myTimer;
    myTimer.start();
    engine->simulate();
    timePhysics = myTimer.elapsed();
}

RigidBody* Scene::createBox(Math::Vector3 &size, Math::Vector3 &position, Math::Quaternion &rotation)
{
    RigidBody *newrb = new RigidBody(RigidBody::BOX, engine->createBox(size,position,rotation));
    newrb->size = size;
    engine->addBody(newrb);
    newrb->position = position;
    newrb->orientation = rotation;
    newrb->initPosition = position;
    newrb->initOrientation = rotation;
    newrb->aabb.setEmpty();
    newrb->aabb.extend(size);
    newrb->aabb.extend(size*-1);
    this->aabb.extend(newrb->aabb);
    selectedBody = newrb;
    return newrb;
}

RigidBody* Scene::createSphere(Math::Real &radius, Math::Vector3 &position)
{
    RigidBody *newrb = new RigidBody(RigidBody::SPHERE, engine->createSphere(radius,position));
    newrb->radius = radius;
    engine->addBody(newrb);
    newrb->position = position;
    newrb->initPosition = position;
    newrb->aabb.setEmpty();
    Math::Vector3 vr(radius,radius,radius);
    newrb->aabb.extend(vr);
    newrb->aabb.extend(vr*-1);
    this->aabb.extend(newrb->aabb);
    selectedBody = newrb;
    return newrb;
}

RigidBody* Scene::createMesh(const QString &name, Math::Vector3 &position, Math::Quaternion &rotation)
{
    TriMesh* mesh = resourceManager->Get(name);
    RigidBody *newrb = new RigidBody(RigidBody::MESH, engine->createMesh(mesh,position,rotation));
    newrb->mesh_filename = name;
    newrb->mesh = mesh;
    engine->addBody(newrb);
    newrb->position = position;
    newrb->initPosition = position;
    newrb->aabb.setEmpty();
    mesh->ComputeBoundingBox();
    newrb->aabb = mesh->BBox();

    selectedBody = newrb;
    return newrb;
}

void Scene::stop(void)
{
    engine->stop();
}

void Scene::selectBody(int id)
{
    std::list<RigidBody*>::iterator it = engine->bodies.begin();
    for(;it!=engine->bodies.end() && (*it)->id != id;++it) {}
    if(it != engine->bodies.end())
    selectedBody = (*it);
}

void Scene::addGround(void)
{
    Math::Vector3 size(50,5,50),position(0,-5,0);
    Math::Quaternion q(true);

    ground = new RigidBody(RigidBody::BOX, engine->createBox(size,position,q));
    ground->size = size;
    engine->addBody(ground);
    ground->position = position;
    ground->orientation = q;
    ground->initPosition = position;
    ground->initOrientation = q;
    ground->aabb.setEmpty();
    ground->aabb.extend(size);
    Math::Vector3 aux(size*-1);
    ground->aabb.extend(size*-1);

    ground->color = Qt::gray;
    engine->setStatic(ground, true);
    ground->isStatic = true;
}

void Scene::save()
{
    QDir dir = QDir::current();
    std::cout << dir.absolutePath().toStdString() << std::endl;
    QString filename = QFileDialog::getSaveFileName(NULL, "Open File",
                                                    QString(rootDir + "scenes/paco.xml").toStdString().c_str(),"*.xml");
    std::ofstream lastSavedFile(QString(rootDir + "scenes/lastSavedFile.txt").toStdString().c_str());

    lastSavedFile << filename.toStdString();
    lastSavedFile.close();

    QFile fout(filename);
    fout.open(QIODevice::WriteOnly);
    QXmlStreamWriter xmlWriter( &fout );
    xmlWriter.setAutoFormatting( true );

    xmlWriter.writeStartDocument();
    xmlWriter.writeStartElement( "Scene" );

    std::stringstream ss;

    ss << engine->timestep;
    xmlWriter.writeAttribute("timestep",ss.str().c_str());
    ss.str("");

    ss << engine->gravity;
    xmlWriter.writeAttribute("gravity",ss.str().c_str());
    ss.str("");

    std::list<RigidBody*>::iterator mi = engine->bodies.begin();
    for( mi; mi != engine->bodies.end(); ++mi )
    {
        if(*mi == ground) continue;
        xmlWriter.writeStartElement("RigidBody");
        xmlWriter.writeAttribute("id",QString::number((*mi)->id));
          switch((*mi)->type)
          {
              case RigidBody::BOX: xmlWriter.writeAttribute("Type", "Box");
                               xmlWriter.writeAttribute("Size",(*mi)->size.toString());
                               break;
              case RigidBody::SPHERE: xmlWriter.writeAttribute("Type","Sphere");
                               xmlWriter.writeAttribute("Size",QString::number((*mi)->radius));
                               break;
              case RigidBody::MESH: xmlWriter.writeAttribute("Type","Mesh");
                               xmlWriter.writeAttribute("File",(*mi)->mesh_filename);
                               break;
          }
          xmlWriter.writeAttribute( "Position" ,(*mi)->initPosition.toString());
          xmlWriter.writeAttribute( "Orientation" ,(*mi)->initOrientation.toString());
          xmlWriter.writeAttribute("Color", (*mi)->color.name());
          xmlWriter.writeAttribute("Static", (*mi)->isStatic ? "true" : "false");
       xmlWriter.writeEndElement();
    }


    xmlWriter.writeEndElement();
    xmlWriter.writeEndDocument();
    fout.close();
    return;
}

void Scene::load(QString filename)
{

    if(filename == "")
        filename = QFileDialog::getOpenFileName(NULL, "Open File",QString(rootDir + "scenes/").toStdString().c_str(),"*.xml");
    std::ofstream lastSavedFile(QString(rootDir + "scenes/lastSavedFile.txt").toStdString().c_str());

    lastSavedFile << filename.toStdString();
    lastSavedFile.close();

    QFile fin(filename);
    fin.open(QIODevice::ReadOnly);
    QXmlStreamReader xmlReader( &fin );

    while(!xmlReader.atEnd() && !xmlReader.hasError()) {
        /* Read next element.*/
        QXmlStreamReader::TokenType token = xmlReader.readNext();
        /* If token is just StartDocument, we'll go to next.*/
        if(token == QXmlStreamReader::StartDocument) {
            continue;
        }
        /* If token is StartElement, we'll see if we can read it.*/
        if(token == QXmlStreamReader::StartElement) {
            /* If it's named persons, we'll go to the next.*/
            if(xmlReader.name() == "Scene") {
                engine->setTimestep(xmlReader.attributes().value("timestep").toString().toFloat());
                engine->setGravity(xmlReader.attributes().value("gravity").toString().toFloat());
                continue;
            }
            /* If it's named person, we'll dig the information from there.*/
            if(xmlReader.name() == "RigidBody") {
                int id = xmlReader.attributes().value("id").toString().toInt();
                Math::Vector3 pos = Math::Vector3::fromString(xmlReader.attributes().value("Position").toString());
                Math::Quaternion rot = Math::Quaternion::fromString(xmlReader.attributes().value("Orientation").toString());
                QColor color(xmlReader.attributes().value("Color").toString());
                bool st = xmlReader.attributes().value("Static").toString().toInt();
                QString type = xmlReader.attributes().value("Type").toString();
                RigidBody* body;
                if (type == "Box")
                {
                    Math::Vector3 s = Math::Vector3::fromString(xmlReader.attributes().value("Size").toString());
                    body = createBox(s,pos,rot);
                }
                else if (type == "Sphere")
                {
                    Math::Real s = xmlReader.attributes().value("Size").toString().toFloat();
                    body = createSphere(s,pos);
                }
                else if (type == "Mesh") {
                    QString name = xmlReader.attributes().value("File").toString();
                    body = createMesh(name,pos,rot);
                }

                body->color = color;
                if(st)
                    engine->setStatic(body,st);
                //body->id = id;
                continue;
            }
        }
    }
    /* Error handling. */
//    if(xmlReader.hasError()) {
//        QMessageBox::critical(this,
//                              "QXSRExample::parseXML",
//                              xmlReader.errorString(),
//                              QMessageBox::Ok);
//    }
    /* Removes any device() or data from the reader
         * and resets its internal state to the initial state. */
    xmlReader.clear();

}

void Scene::removeSelectedBody(void)
{
    engine->removeRigidBody(selectedBody);
    std::list<RigidBody*>::iterator it = engine->bodies.begin();
    for(;it!= engine->bodies.end() && *it != selectedBody;++it) {}
    if(it!=engine->bodies.end())
        engine->bodies.erase(it);
}

void Scene::deleteScene()
{
    std::list<RigidBody*>::iterator it = engine->bodies.begin();
    for(;it!=engine->bodies.end();++it)
    {
        if(*it == ground) continue;
        engine->removeRigidBody(*it);
        //engine->bodies.erase(it);
    }
    it = engine->bodies.begin();
    it++;
    engine->bodies.erase(it,engine->bodies.end());
}
